#include <flight_controller_udp_fdata.hpp>
#include <msg_flight_data_s.hpp>
#include <msg_param_s.hpp>
#include <rtime.hpp>

FlightControllerUdpFData::FlightControllerUdpFData()
{
	m_BuffRecv = (char*)malloc(BUFF_FIFO_SIZE);
}

FlightControllerUdpFData::~FlightControllerUdpFData()
{
	free(m_BuffRecv);
}

void FlightControllerUdpFData::RecvData(char* data, int size)
{
	m_BuffFifo.Append(data, size);
	while (1)
	{
		if (m_Protocol.Unpack(m_BuffRecv, &m_SizeRecv, &m_DataSeqRecv, &m_DataTypeRecv, &m_BuffFifo) != 0)
		{
			break;
		}

		switch (m_DataTypeRecv)
		{
			case P_TYPE_FLIGHT_DATA:
				if (m_SizeRecv == sizeof(msg_flight_data_s))
				{
					static int fd	  = -1;
					static char* buff = NULL;
					if (fd < 0)
					{
						fd	 = open("/data/temp/1.txt", O_CREAT | O_WRONLY | O_TRUNC, 0666);
						buff = (char*)malloc(1000);
					}
					msg_flight_data_s* p = (msg_flight_data_s*)m_BuffRecv;

					// int len = snprintf(buff, 1000, "%+.6lf %+.6lf %+.6lf %+.6lf\n", p->velocity_setpoint.vel[2], p->attitude.z_vel, p->sensors.gyro[2], p->yaw_rate_setpoing.yaw_rate_setpoint);
					int len = snprintf(buff, 1000, "%+.6lf\n", p->attitude.angles[2]);
					int ret = write(fd, buff, len);
					if (ret > 0)
					{
						fsync(fd);
					}
					else
					{
						close(fd);
						fd = -1;
						free(buff);
					}
					// printf("%+8.4lf %+8.4lf %+8.4lf\n", p->attitude.angles[0], p->attitude.angles[1], p->attitude.angles[2]);
					// printf("%+8.4lf %+8.4lf ", p->velocity_setpoint.vel[2], p->attitude.z_vel);
					printf("%+8.4lf %+8.4lf\n", p->sensors.gyro[2], p->yaw_rate_setpoing.yaw_rate_setpoint);
					__UNUSED(p)
				}
				break;

			default:
				break;
		}
	}
}